//#include "xavi.h"

void avanzaHastaChocar(unsigned char distChoque) {
	LED_TOP_GREEN_ON();
	while(!(SEN_OBS_ANALOG(OBS_CENTER_L)>distChoque
		&& SEN_OBS_ANALOG(OBS_CENTER_R)>distChoque)){
		MOT_STR(100, FWD, TIME, distChoque);
	};
	MOT_STR(100, FWD, TIME, distChoque);
	while(!MOT_END){};
	LED_TOP_GREEN_OFF();
}

void avanzaHastaHueco(unsigned char distChoque) {
	LED_TOP_GREEN_ON();
	LED_FRONT_ON();
	while(!SEN_OBS_ANALOG(OBS_SIDE_R)>distChoque){
		MOT_STR(100, FWD, TIME, distChoque);
	};
	MOT_STR(100, FWD, TIME, distChoque);
	while(!MOT_END){};
	LED_TOP_GREEN_OFF();
	LED_FRONT_OFF();
}


void aparcaSobreLaIzda() {
	LED_TOP_RED_ON();
	MOT_ROT(50, BACK, CENTER, LEFT, ANGLE, 25) ;
	while(!MOT_END){};
	MOT_STR(100, BACK,TIME,0);
	while(MOT_FDBCK(STATUS_T)[0]<5);
	MOT_STOP();
	LED_TOP_RED_OFF();	
}

void rectifica(unsigned char ultDerecha){
	if (SEN_OBS_ANALOG(OBS_SIDE_R) > ultDerecha){
		MOT_ROT(1, BACK, CENTER, RIGHT, ANGLE, 1) ;
		while(!MOT_END){};
	} else if (SEN_OBS_ANALOG(OBS_SIDE_R) < ultDerecha) {
		MOT_ROT(1, BACK, CENTER, LEFT, ANGLE, 1) ;
		while(!MOT_END){};
	}
	ultDerecha = SEN_OBS_ANALOG(OBS_SIDE_R);
}

void xavi() {
	unsigned char ultDerecha = SEN_OBS_ANALOG(OBS_SIDE_R);
	while(1){
		avanzaHastaHueco(10);
		//rectifica(ultDerecha);
		aparcaSobreLaIzda();
	}
}